#include "Communication/SubOdom.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller {
OdomData DataSet::odom_data;
namespace SubOdom {

static rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSub;

void sub_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
    OdomData odom_data(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->twist.twist.linear.x,
                       msg->twist.twist.angular.z, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y,
                       msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
    DataSet::odom_data = odom_data;
}

void SubOdomInfo(rclcpp::Node::SharedPtr &node_handle) {
    odomSub = node_handle->create_subscription<nav_msgs::msg::Odometry>(
            "odom", rclcpp::SensorDataQoS(), sub_callback);
    std::cout << "SubOdom SubOdomInfo" << std::endl;
}
} // namespace SubOdom

} // namespace behavior_controller